#ifndef ANDRES_OUTPUT_OUTPUT_IMPL_H
#define ANDRES_OUTPUT_OUTPUT_IMPL_H

#include <opencv2/opencv.hpp>
#include <base/defines.h>
#include <base/common.h>
#include <base/camera.h>
#include <string>

#include "output.h"

class DbgFrameImpl : public DbgFrame
{
public:
    DbgFrameImpl(const std::string& img_path, const GeoPoint& gps_point, Camera* camera, double range = 100.0);
    virtual ~DbgFrameImpl();
    void DrawTrcPoints(const std::vector<GeoPoint>& gps_points, const cv::Scalar& color) override;
    void PrintIntoImg(const std::string& filename) override;
    void DrawHDmapProjectView(const hdmap_frame& hdmap_f, const Pose& pose, const cv::Scalar& color);
    void DrawParticles(Filtering::ParticleFilter* pf) override;
    void DrawSolidLine3D(const std::vector<Eigen::Vector3d>& spline, const cv::Scalar& color, int line_width) override;
    void DrawRound2D(const Eigen::Vector2d& center, const cv::Scalar& color, double radius) override;
    void DrawRectangle2D(const envelope2D& env, const cv::Scalar& color, double line_width) override;
    void DrawSolidLine2D(const std::vector<Eigen::Vector2d>& spline, const cv::Scalar& color, int line_width) override;
    void DrawHorizontial(const GeoPoint& gps_point,const std::vector<hdmap_lane> &hdlanes);
    void DrawDlFrame2D(const DlFrame& dlFrame, const cv::Scalar& color) override;
    cv::Mat GetTopMat() const;
    cv::Mat GetBottomMat() const;
protected:
    void DrawArrow(const Eigen::Vector3d& center, double bearing, const cv::Scalar &color);
private:
    Eigen::Vector2d toCanvas(double x, double y);
 
private:
    double xscale_;
    double yscale_;
    double col_;
    double row_;
    cv::Mat top_mat_;
    cv::Mat bottom_mat_;
    cv::Mat combined_mat_;
    Eigen::Vector2d min_;
    Eigen::Vector2d max_;
    Camera* camera_ptr_;
};

#endif